/*
 *	wiiuse
 */

/**
 *	@file
 *
 *	@brief WiiMote events manager
 *
 */

#include <stdio.h>
#include <stdlib.h>
#include <pthread.h>
#include "wiiuse.h"
#include "WiiHelperStructs.h"
#include "WiiEventHandler.h"
#include "WiimoteManager.h"

#ifndef WIN32
	#include <unistd.h>
#endif

wiimote** wiimotes; //Main structure for wiimotes
wmCallbacks *wmFunctions; //Main structure for wiimote functiosn pointers
int MAX_WIIMOTES = 4; //Max Number of wiimotes allowed
int WMSEARCHTIME = 2000, //Default searching time for a new wiimote connection
    WMCONNECTED = 0 ; //Number of wiimotes actually connected

wiimoteStatus** wiimotesStatus; //Helps tp know if the motion sensor is active for every wiimote

/**
 *     @brief Run the main loop for the wiimote listener
 * 
 */
int wiiListenerMainLoop(void *retrollamada){

        wmCallbacks *functions = malloc(sizeof(wmCallbacks));
        functions = retrollamada;
    	/*
	 *	This is the main loop
	 *
	 *	wiiuse_poll() needs to be called with the wiimote array
	 *	and the number of wiimote structures in that array
	 *	(it doesn't matter if some of those wiimotes are not used
	 *	or are not connected).
	 *
	 *	This function will set the event flag for each wiimote
	 *	when the wiimote has things to report.
	 */
	while (1) {
		if (wiiuse_poll(wiimotes, MAX_WIIMOTES)) {
			/*
			 *	This happens if something happened on any wiimote.
			 *	So go through each one and check if anything happened.
			 */
			int i = 0;
			for (; i < MAX_WIIMOTES; ++i) {
				switch (wiimotes[i]->event) {
					case WIIUSE_EVENT:
						/* a generic event occured */
						handle_event(wiimotes[i], i, functions);
						break;

					case WIIUSE_STATUS:
						/* a status event occured */
						handle_ctrl_status(wiimotes[i]);
						break;

					case WIIUSE_DISCONNECT:
					case WIIUSE_UNEXPECTED_DISCONNECT:
						/* the wiimote disconnected */
						handle_disconnect(wiimotes[i]);
						break;

					case WIIUSE_READ_DATA:
						/*
						 *	Data we requested to read was returned.
						 *	Take a look at wiimotes[i]->read_req
						 *	for the data.
						 */
						break;

					case WIIUSE_NUNCHUK_INSERTED:
						/*
						 *	a nunchuk was inserted
						 *	This is a good place to set any nunchuk specific
						 *	threshold values.  By default they are the same
						 *	as the wiimote.
						 */
						 //wiiuse_set_nunchuk_orient_threshold((struct nunchuk_t*)&wiimotes[i]->exp.nunchuk, 90.0f);
						 //wiiuse_set_nunchuk_accel_threshold((struct nunchuk_t*)&wiimotes[i]->exp.nunchuk, 100);
						printf("Nunchuk inserted.\n");
						break;

					case WIIUSE_CLASSIC_CTRL_INSERTED:
						printf("Classic controller inserted.\n");
						break;

					case WIIUSE_GUITAR_HERO_3_CTRL_INSERTED:
						/* some expansion was inserted */
						handle_ctrl_status(wiimotes[i]);
						printf("Guitar Hero 3 controller inserted.\n");
						break;

					case WIIUSE_NUNCHUK_REMOVED:
					case WIIUSE_CLASSIC_CTRL_REMOVED:
					case WIIUSE_GUITAR_HERO_3_CTRL_REMOVED:
						/* some expansion was removed */
						handle_ctrl_status(wiimotes[i]);
						printf("An expansion was removed.\n");
						break;

					default:
						break;
				}
			}
                }
	}
    free(functions);
    return 0;
}

/*
 * Enable/disable wiimote motion given the id
 */
void toogleWMMotion(int wmID){
    //Todo hacer comprobacion de que el wiimote existe
    if(wiimotesStatus[wmID]->toogleMotion == 1){
        wiiuse_motion_sensing(wiimotes[wmID], 0);
        wiimotesStatus[wmID]->toogleMotion = 0;
    }else{
        wiiuse_motion_sensing(wiimotes[wmID], 1);
        wiimotesStatus[wmID]->toogleMotion = 1;
    }
}

/*
 * Enable/disable wiimote rumble given the id
 */
void toogleWMRumble(int wmID){
    //TODo comprobar que el wiimote existe
    wiiuse_toggle_rumble(wiimotes[wmID]);
}

void toogleWMIRTrackin(int wmID, int status){
    //Todo comprobar que el wiimote existe
    if (status == 1)
        wiiuse_set_ir(wiimotes[wmID], 1);
    else if (status == 0)
        wiiuse_set_ir(wiimotes[wmID], 0);
}

/*
 * Return wiimote battery level
 */
float getWMBatteryStatus(int wmID){
    return wiimotes[wmID]->battery_level;
}

/*
 * Return the actual number of connected wiimotes
 */
int getNumberOfConnectedWM(){
    return WMCONNECTED;
}

/**
 *     @brief configure wiimote connection parameters
 * 
 *      This functio just  nedd to be called once
 */
void confWMCon(int maxWiimotesAllowed, int searchTime){

    MAX_WIIMOTES = maxWiimotesAllowed;
    WMSEARCHTIME = searchTime;

    /*
     *	Initialize an array of wiimote objects.
     *	The parameter is the number of wiimotes I want to create.
     */
    wiimotes =  wiiuse_init(MAX_WIIMOTES);

    /*
     *  Finds the Bluuetooth windows stack and make use of it
     */
    #ifdef WIN32
        wiiuse_set_bluetooth_stack(wiimotes, MAX_WIIMOTES, WIIUSE_STACK_MS);
    #endif

}

/**
 *     @brief configure wiimote connection parameters
 *
 *      This functio just  nedd to be called once
 */
int foundAndSetupWM(){

    int wmFound;
    int i = 0;


    	/*
	 *	Find wiimote devices
	 *
	 *	Now we need to find some wiimotes.
	 *	Give the function the wiimote array we created, and tell it there
	 *	are MAX_WIIMOTES wiimotes we are interested in.
	 *
	 *	Set the timeout to be 5 seconds.
	 *
	 *	This will return the number of actual wiimotes that are in discovery mode.
	 */
    wmFound = wiiuse_find(wiimotes, MAX_WIIMOTES, 5);

    //TODO add support for new wiimotes conected just in time, threads?
    /*
     * Keep searching for connected wiimotes
     */
    while(!wmFound){
        printf ("No wiimotes found. ");
        printf ("Searching again in %d miliseconds...\n", WMSEARCHTIME);
        Sleep(WMSEARCHTIME);
        wmFound = wiiuse_find(wiimotes, MAX_WIIMOTES, 5); //Timeout 5, can can change to wait moretime for wiimote answer
    }

    /*
     *	Connect to the wiimotes
     *
     *	Now that we found some wiimotes, connect to them.
     *	Give the function the wiimote array and the number
     *	of wiimote devices we found.
     *
     *	This will return the number of established connections to the found wiimotes.
     */
    WMCONNECTED = wiiuse_connect(wiimotes, MAX_WIIMOTES);
    if (WMCONNECTED){
        printf("Connected to %i wiimotes (of %i found).\n", WMCONNECTED, wmFound);

        wiimotesStatus = malloc(sizeof(wiimoteStatus) * WMCONNECTED);
        for(; i < WMCONNECTED; i++){
            wiimotesStatus[i] = malloc(sizeof(wiimoteStatus));
           // wiimotesStatus[i]->wmID = i; //TODO Parece innecesario si se usan los ids el array en su lugar
            wiimotesStatus[i]->toogleMotion = 0;
        }

        return 0;
    }else {
        printf("Failed to connect to any wiimote.\n");
        return 1;
    }

}

/**
 *  @brief Send a signal to avery wiimote to check the connection
 *  Default time must be 200ms
 */
int wmStartSignal(int rumbleTime){
    	/*
	 *	Set the LEDs and rumble for a second so it's easy
	 *	to tell which wiimotes are connected (just like the wii does).
	 */
	wiiuse_set_leds(wiimotes[0], WIIMOTE_LED_1);
	wiiuse_set_leds(wiimotes[1], WIIMOTE_LED_2);
	wiiuse_set_leds(wiimotes[2], WIIMOTE_LED_3);
	wiiuse_set_leds(wiimotes[3], WIIMOTE_LED_4);

        /*
         * Turn on the rumble wiimote function
         */
	wiiuse_rumble(wiimotes[0], 1);
	wiiuse_rumble(wiimotes[1], 1);
        wiiuse_rumble(wiimotes[2], 1);
	wiiuse_rumble(wiimotes[3], 1);

        #ifndef WIN32
		usleep(rumbleTime * 1000);
	#else
		Sleep(rumbleTime);
	#endif

        /*
         * Turn off the wiimote rumble function
         */
	wiiuse_rumble(wiimotes[0], 0);
	wiiuse_rumble(wiimotes[1], 0);
        wiiuse_rumble(wiimotes[2], 0);
        wiiuse_rumble(wiimotes[3], 0);

}

void wmCleanup(){
    wiiuse_cleanup(wiimotes, MAX_WIIMOTES);
}


//TODO documentar adecuadamente estas funciones
/*
 * 	@brief wmButtonFunc(int (*retroLlamada)(int wmButton, int wmID))
 *      Save a pointer to the callback function
 */
void wmButtonPressedFunc(int (*retroLlamada)(int wmButton, int wmID)){
    wmFunctions->wmButtonPressedAction = retroLlamada;
}

void wmButtonReleasedFunc(int (*retrollamada)(  int wmButton /*Constant to specify the released wiimote code button*/,
                                                int wmID /*Wiimote ID*/)){
    wmFunctions->wmButtonReleasedAction = retrollamada;
}

/*
 * 	@brief wmButtonFunc(int (*retroLlamada)(int wmButton, int wmID))
 *      Save a pointer to the callback function
 */
void wmAccelerometerFunc(int (*retroLlamada)(float roll,
                                    float pitch	/* pitch angle*/,
                                    float yaw /*yaw angle*/,
                                    int wmID /*Wiimmote ID*/)){
    wmFunctions->wmAccelerometerAction = retroLlamada;
}

/*
 * 	@brief wmButtonFunc(int (*retroLlamada)(int wmButton, int wmID))
 *      Save a pointer to the callback function
 */
void wmIRFunc(int (*retroLlamada)(  int x /* X virtual screen coordinate*/,
                                    int y /* Y virtual screen coordinate*/,
                                    int point /* Point number localized*/,
                                    int wmID /*Wiimmote ID*/)){
    wmFunctions->wmIRAction = retroLlamada;
}

/*
 *	@brief enableWMSupport()
 *      Setup the inicital wiimote configuration
 *
 */
void enableWMSupport(){
        confWMCon(4, 2000); //Configure Wm connection
        foundAndSetupWM(); //Found and setup wms connections
        wmStartSignal(200); //Send a brief rumble signal to every wiimote
        wmFunctions = malloc(sizeof(wmCallbacks));
        wmFunctions->wmAccelerometerAction = NULL;
        wmFunctions->wmButtonPressedAction = NULL;
        wmFunctions->wmButtonReleasedAction = NULL;
        wmFunctions->wmIRAction = NULL;
}

/**
 *	@brief initWiiListening()
 *
 *	Connect to up to two wiimotes and print any events
 *	that occur on either device.
 */
int initWiimoteListening() {

        //TODO meter soporte para control de versiones con GitHub

/*
        confWMCon(4, 2000); //Configure Wm connection
        foundAndSetupWM(); //Found and setup wms connections
        wmStartSignal(200); //Send a brief rumble signal to every wiimote
*/


	/*
	 *	Maybe I'm interested in the battery power of the 0th
	 *	wiimote.  This should be WIIMOTE_ID_1 but to be sure
	 *	you can get the wiimote assoicated with WIIMOTE_ID_1
	 *	using the wiiuse_get_by_id() function.
	 *
	 *	A status request will return other things too, like
	 *	if any expansions are plugged into the wiimote or
	 *	what LEDs are lit.
	 */
	//wiiuse_status(wiimotes[0]);
        
        /*
         * Crea un hilo para escuchar eventos de los wiimotes
         */
        pthread_t tid[1]; // threads IDs

/*
        wiiMoteControllerHelper *wiiMoteControls = malloc(sizeof(wiiMoteControllerHelper));
        wiiMoteControls->wiiRemoteControls = wiimotes;
*/
        //TODO esto de la estructura de punteros a funciones, si jala, ahora a implementar mas funciones para los demas eventos
/*
        wmCallbacks *wmFunctions = malloc(sizeof(wmCallbacks));
        wmFunctions->wmButtonAction = retroLlamada;
*/
        
        pthread_create(&tid[0], NULL, wiiListenerMainLoop, (void *) wmFunctions);
        
        #ifdef DEBUG
            printf("Esperando la finalizacion de glut\n");
            fflush(stdout);
        #endif

       //It's not need to call this function, unless we dont want glut to continue running
       // pthread_join(tid[0], NULL);
        
	/*
	 *  Disconnect the wiimotes
	 */
	
	return 0;
}
